#include "taskintf_config.h"
#include "axisgroup/axisgroup_set.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "axisgroup/ag_config.h"
#include "motion_al.h"
#include "log/zuclog.h"
#include "axisgroup/pfsm_servo_filter.h"
#include "safety/safety_cmd.h"
#include "axisgroup/force_control.h"
#include <mutex>

// 本文件的接口，操作axisgroup内的set_intf接口，设置内容至用户配置文件，实际落盘时刻为上下电，上下使能；
// 该配置的生效，为每个周期motion层更新激活的配置
// 该配置的操作，部分无额外操作的加锁写
// 部分有额外操作的使用cmd机制

#define TASKCONFIG_INFO(fmt, ...) zuclog_info("TASKCONFIG %d", fmt, robot_id, ##__VA_ARGS__)
#define TASKCONFIG_ERR(fmt, ...) zuclog_error("TASKCONFIG %d", fmt, robot_id, ##__VA_ARGS__)
#define TASKCONFIG_DEBUG(fmt, ...) zuclog_debug("TASKCONFIG %d", "[%06d] " fmt, robot_id, __LINE__, ##__VA_ARGS__)

#define SET_CONFIG(PART)                                                                             \
    do {                                                                                             \
        auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf(); \
        auto config = set_intf->get_usr_config();                                                    \
        set_intf->config_change();                                                                   \
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);                                   \
        PART                                                                                         \
    } while (0)

#define INVALID_PARA -2

void write_robot_config_to_file()
{
    for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
    {
        auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
        robot->get_set_intf()->write_usr_config_to_file();
    }
}

int check_config_active(int robot_id)
{
    if (robot_id < 0 || robot_id >= (int)mot::al::MotionProxy::instance().robot_cnt())
    {
        TASKCONFIG_ERR("robot_id %d out of range", robot_id);
        return -1;
    }
    return 0;
    //伟创力定制的手自动模式功能开启，且在自动模式下，禁止设置安全参数 || 正在运动中则禁止设置安全参数
    // if ((zucStatus->operation_mode_option == 1 && zucStatus->operation_mode == 1) || zucStatus->motion.traj.inpos != 1)
    // {
    //     zucOperatorError(NONMANUAL_CANNOT_SET_ROBOT_SAFETY_PARAMETERS,
    //                      "The safety parameters cannot be configured if the robot is not in manual mode or in manual mode motion.");
    //     return -1;
    // }
    // //如果程序执行则禁止设置安全参数
    // if (zucStatus->task.interpState == ZUC_TASK_INTERP_READING || zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED ||
    //     zucStatus->task.interpState == ZUC_TASK_INTERP_WAITING)
    // {
    //     zucOperatorError(PROGRAM_EXECUTE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the program is being executed.");
    //     return -1;
    // };
}

int check_joint_config_active(int robot_id, int joint)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    auto max_joint = mot::al::MotionProxy::instance().robot(robot_id)->get_status().robot_joint_num;
    if (joint < 0 || joint >= max_joint)
    {
        TASKCONFIG_ERR("joint %d out of range", joint);
        return -1;
    }
    return 0;
}

// cmd 设置
int zucJointSetMinPositionLimit(int robot_id, int joint, double minLimit)
{
    auto cmd = std::make_shared<CmdSetJointMinLimits>(robot_id, joint, minLimit);
    int retval = mot::realtime_cmd_call(cmd);
    return retval;
}

// cmd 设置
int zucJointSetMaxPositionLimit(int robot_id, int joint, double limit)
{
    auto cmd = std::make_shared<CmdSetJointMaxLimits>(robot_id, joint, limit);
    int retval = mot::realtime_cmd_call(cmd);
    return retval;
}

// cmd 设置
int zucJointSetMaxVelocity(int robot_id, int joint, double vel)
{
    auto cmd = std::make_shared<CmdSetJointVelLimit>(robot_id, joint, vel);
    int retval = mot::realtime_cmd_call(cmd);
    return retval;
}

// cmd 设置
int zucJointSetErrLimit(int robot_id, int joint, double err_limit)
{
    auto cmd = std::make_shared<CmdSetJointErrLimit>(robot_id, joint, err_limit);
    int retval = mot::realtime_cmd_call(cmd);
    return retval;
}

// cmd 设置
int zucSetJointSafety(int robot_id, int jointNum, int paramType, float joint_vel_limit, float joint_err_limit)
{
    if (paramType == 1)
    {
        zucJointSetMaxVelocity(robot_id, jointNum, joint_vel_limit);
    }
    if (paramType == 2)
    {
        zucJointSetErrLimit(robot_id, jointNum, joint_err_limit);
    }
    return 0;
}

// 锁 设置 --- 回零
int zucSetCustomPose(int robot_id, int type, const double* jointPos)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    TASKCONFIG_INFO("ZUCMOT_SET_INIT_POSE\n");
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();
    if (type == 0)
    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        // shrunken pose
        for (int i = 0; i < status.robot_joint_num; i++) { config->default_pose.shrunken[i] = jointPos[i]; }
        set_intf->config_change();
    }
    else if (type == 1)
    {
        // stretched pose
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        for (int i = 0; i < status.robot_joint_num; i++) { config->default_pose.stretched[i] = jointPos[i]; }
        set_intf->config_change();
    }
    else if (type == 2)
    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        // init pose
        for (int i = 0; i < status.robot_joint_num; i++) { config->default_pose.initial[i] = jointPos[i]; }
        set_intf->config_change();
    }

    // double oripos[ZUCMOT_MAX_JOINTS];
    // int cmpsame = 0;
    // for (int joint_num_ = 0; joint_num_ < zucmotConfig->numJoints; joint_num_++)
    // {
    //     if (zucmotConfig->safetyCfg.init_traj_jpos[joint_num_] == trajInitPosj_[joint_num_])
    //         cmpsame++;
    //     oripos[joint_num_] = zucmotConfig->safetyCfg.init_traj_jpos[joint_num_];
    //     zucmotConfig->safetyCfg.init_traj_jpos[joint_num_] = trajInitPosj_[joint_num_];
    //     MOTCMD_INFO("init pos %d: %f\n", joint_num_, zucmotConfig->safetyCfg.init_traj_jpos[joint_num_]);
    // }
    // unsigned int ori_crc = zucmotStatus->safetyCRC;
    // zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
    // // @FIXME liwang 此处checksum信息未考虑7轴
    // if (cmpsame != 6 && task_init_finish_flag == 1)
    // {
    //     char str[MAX_ERROR_MSG_STR_LEN];
    //     snprintf(str,
    //              MAX_ERROR_MSG_STR_LEN,
    //              "{\"fixed_key\":[\"%#x\", \"%#x\",\"%.3f;%.3f;%.3f;%.3f;%.3f;%.3f\",\"%.3f;%.3f;%.3f;%.3f;%.3f;%.3f\"]}",
    //              ori_crc,
    //              zucmotStatus->safetyCRC,
    //              oripos[0],
    //              oripos[1],
    //              oripos[2],
    //              oripos[3],
    //              oripos[4],
    //              oripos[5],
    //              zucmotConfig->safetyCfg.init_traj_jpos[0],
    //              zucmotConfig->safetyCfg.init_traj_jpos[1],
    //              zucmotConfig->safetyCfg.init_traj_jpos[2],
    //              zucmotConfig->safetyCfg.init_traj_jpos[3],
    //              zucmotConfig->safetyCfg.init_traj_jpos[4],
    //              zucmotConfig->safetyCfg.init_traj_jpos[5]);
    //     reportInfo(INFO_SAFETY_CONF_INIT_POSE_CHANGED, "checksum changed # %s ", str);
    // }
    return 0;
}

// 锁 设置 --- 回零
int zucSetHomeVel(int robot_id, double vel)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    if (vel < 0 || vel > 100)
    {
        TASKCONFIG_ERR("Invalid home velocity %f\n", vel);
        return INVALID_PARA;
    }
    TASKCONFIG_INFO("ZUCMOT_SET_HOME_VEL, %lf\n", vel);
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->default_pose.vel_scale = vel;  // 回零速度百分比
        set_intf->config_change();
    }
    return 0;
}

// 锁 设置
int zucSetCustomPoseTol(int robot_id, double tol)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (tol < 0.001 || tol > 3)
    {
        TASKCONFIG_ERR("Invalid init pose tolerance %f\n", tol);
        return INVALID_PARA;
    }
    int ret = 0;
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->default_pose.tol = tol;
        set_intf->config_change();
    }

    // double orig_tol;
    // orig_tol = zucmotConfig->safetyCfg.init_jpos_tol;
    // zucmotConfig->safetyCfg.init_jpos_tol = tol_;
    // MOTCMD_INFO("ZUCMOT_SET_INIT_POSE_TOL: %f\n", zucmotConfig->safetyCfg.init_jpos_tol);
    // unsigned int ori_crc = zucmotStatus->safetyCRC;
    // zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
    // if (orig_tol != zucmotConfig->safetyCfg.init_jpos_tol && task_init_finish_flag == 1)
    // {
    //     char str[MAX_ERROR_MSG_STR_LEN];
    //     snprintf(str,
    //              MAX_ERROR_MSG_STR_LEN,
    //              "{\"fixed_key\":[\"%#x\", \"%#x\", \"%.3f\",\"%.3f\"]}",
    //              ori_crc,
    //              zucmotStatus->safetyCRC,
    //              orig_tol,
    //              zucmotConfig->safetyCfg.init_jpos_tol);
    //     reportInfo(INFO_SAFETY_CONF_INIT_POSE_TOL_CHANGED, "checksum changed # %s ", str);
    // }
    return 0;
}

// 锁 设置 --- servo 参数的配置，仅在切换到servo模式时，才会真正生效
int zucSetServoMoveFilterType(int robot_id, int filterType)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (filterType < 0 || filterType > axisgroup::ServoFilterIntf::FilterType::NLF_MMF_COMB)
    {
        TASKCONFIG_ERR("Invalid filter type %d\n", filterType);
        return INVALID_PARA;
    }
    TASKCONFIG_INFO("ZUCMOT_SET_SERVO_MOVE_FILTER_TYPE: %d\n", filterType);
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->servo_move.filter_type = filterType;
        set_intf->config_change();
    }

    return 0;
}

// 锁 设置 --- servo 参数的配置，仅在切换到servo模式时，才会真正生效
int zucSetServoMoveLPFParam(int robot_id, double cutoffFreq)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (cutoffFreq < 0)
    {
        TASKCONFIG_ERR("Invalid cutoff frequency %f\n", cutoffFreq);
        return INVALID_PARA;
    }
    TASKCONFIG_INFO("ZUCMOT_SET_SERVO_MOVE_LPF_PARAM: %f\n", cutoffFreq);
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->servo_move.lpf_cf = cutoffFreq;
        set_intf->config_change();
    }

    return 0;
}

// 锁 设置 --- servo 参数的配置，仅在切换到servo模式时，才会真正生效
int zucSetServoMoveNLFParamC(int robot_id, double vp, double ap, double jp, double vr, double ar, double jr)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }

    TASKCONFIG_INFO("ZUCMOT_SET_SERVO_MOVE_NLF_PARAM: %f %f %f %f %f %f\n", vp, ap, jp, vr, ar, jr);
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->servo_move.nlf_ap = ap;
        config->servo_move.nlf_jp = jp;
        config->servo_move.nlf_vp = vp;
        config->servo_move.nlf_vr = vr;
        config->servo_move.nlf_ar = ar;
        config->servo_move.nlf_jr = jr;
        set_intf->config_change();
    }

    return 0;
}

// 锁 设置 --- servo 参数的配置，仅在切换到servo模式时，才会真正生效
int zucSetServoMoveNLFParamJ(int robot_id, double vr, double ar, double jr)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }

    TASKCONFIG_INFO("ZUCMOT_SET_SERVO_MOVE_NLF_PARAM: %f %f %f\n", vr, ar, jr);
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->servo_move.nlf_vr = vr;
        config->servo_move.nlf_ar = ar;
        config->servo_move.nlf_jr = jr;
        set_intf->config_change();
    }

    return 0;
}

// 锁 设置 --- servo 参数的配置，仅在切换到servo模式时，才会真正生效
int zucSetServoMoveMMFParam(int robot_id, int maxSize, double ka, double kv, double kp)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }

    TASKCONFIG_INFO("ZUCMOT_SET_SERVO_MOVE_MMF_PARAM: %d %f %f %f\n", maxSize, ka, kv, kp);
    auto status = mot::al::MotionProxy::instance().robot(robot_id)->get_status();  // 当前状态
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->servo_move.mmf_max_buf = maxSize;
        config->servo_move.mmf_ka = ka;
        config->servo_move.mmf_kv = kv;
        config->servo_move.mmf_kp = kp;
        set_intf->config_change();
    }
    return 0;
}

// 锁 设置 --- 力控
int zucAdmittanceDragDeadZone(int robot_id, const double* dead_zone)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    for (int i = 0; i < 6; i++)
    {
        if (dead_zone[i] < 0)
        {
            TASKCONFIG_ERR("Invalid dead zone %d, %f\n", i, dead_zone[i]);
            return INVALID_PARA;
        }
    }
    TASKCONFIG_INFO(
        "ZUCMOT_SET_ADMITTANCE_DRAG_DEAD_ZONE: %f %f %f %f  %f %f\n", dead_zone[0], dead_zone[1], dead_zone[2], dead_zone[3], dead_zone[4], dead_zone[5]);
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        for (int i = 0; i < CART_DIM; i++) { config->force_control.drag_dead_zone[i] = dead_zone[i]; }
        set_intf->config_change();
    }
    return 0;
}

// 锁 设置 --- 力控
int zucTrajSetAdmittanceCtrlFrame(int robot_id, int ftFrame)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (ftFrame < 0 || ftFrame > 1)
    {
        TASKCONFIG_ERR("Invalid ft frame %d\n", ftFrame);
        return INVALID_PARA;
    }
    TASKCONFIG_INFO("ZUCMOT_SET_ADMITTANCE_CTRL_FRAME: %d\n", ftFrame);  // 设置力控坐标系号
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->force_control.admit_fc_frame = (FCAxisPara::FRAME)ftFrame;
        set_intf->config_change();
    }
    return 0;
}

// 锁 设置 --- 力控
int zucTrajSetAdmittanceConfig(int robot_id, int axis, FtConfig ftConfig, int immediate)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (axis < 0 || axis >= CART_DIM)
    {
        TASKCONFIG_ERR("Invalid axis %d\n", axis);
        return INVALID_PARA;
    }
    //    //use zucmotStatus instead to get realtime info, complianttype is not updated into zucStatus
    //    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_ADMITTANCE)
    //    {
    //        zucOperatorError(ERR_SET_ADMITTANCE_WHEN_FC_ON, "It's not allowed to change parameters while force control is still on.");
    //        return -1;
    //    }
    //    if (zucmotStatus.compliantType)
    //    {
    //        if (ftConfig.enableFt == 0 && zucStatus->motion.admitFtConfig[axis].enableFt)
    //        {
    //            zucOperatorError(ERR_REDUCE_ADMITTANCE_WHEN_FC_ON, "It's not allowed to ruduce force control directions while force control is still on.");
    //            return -1;
    //        }
    //    }

    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->force_control.admit_fc_config[axis].enableFt = ftConfig.enableFt;
        config->force_control.admit_fc_config[axis].ftUser = ftConfig.ftUser;
        config->force_control.admit_fc_config[axis].ftReboundFK = ftConfig.ftReboundFK;
        config->force_control.admit_fc_config[axis].ftConstant = ftConfig.ftConstant;
        config->force_control.admit_fc_config[axis].enableNTrack = ftConfig.enableNTrack;
        set_intf->config_change();
    }
    return 0;
}

// 锁 设置 --- 力控
int zucTorqueSensorSoftLimit(int robot_id, const double* limit)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    for (int i = 0; i < CART_DIM; i++)
    {
        if (limit[i] < 0)
        {
            TASKCONFIG_ERR("Invalid torque limit %d, %f\n", i, limit[i]);
            return INVALID_PARA;
        }
    }
    TASKCONFIG_INFO("ZUCMOT_SET_TORQUE_SENSOR_SOFT_LIMIT: %f %f %f %f %f %f\n", limit[0], limit[1], limit[2], limit[3], limit[4], limit[5]);
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        for (int i = 0; i < CART_DIM; i++) { config->force_control.torque_sensor_limit[i] = limit[i]; }
        set_intf->config_change();
    }

    return 0;
}

// 锁 设置 --- 力控
int zucTorqueSensorFilter(int robot_id, double torqueSensorFilter)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (torqueSensorFilter < 0)
    {
        TASKCONFIG_ERR("Invalid torque sensor filter %f\n", torqueSensorFilter);
        return INVALID_PARA;
    }

    TASKCONFIG_INFO("ZUCMOT_SET_TORQUE_SENSOR_FILTER: %f\n", torqueSensorFilter);

    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->force_control.torque_sensor_filter = torqueSensorFilter;
        set_intf->config_change();
    }

    return 0;
}

// cmd 设置 --- 力控
int zucTrajSetComplianceInit(int robot_id, int enable, int init)
{
    //    // compliantEnable, 1, 初始化，0，不初始化
    //    zucStatus->motion.compliantType = compliantType;
    //    zucStatus->motion.compliantEnable = compliantEnable;
    //
    //    int enable = 0;
    //    if (compliantType)
    //    {
    //        enable = 1;
    //    }
    //    auto cmd = std::make_shared<CmdSetCompliantType>(robot_id, enable, compliantType, compliantEnable);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

// cmd 设置 --- collision
int zucSetCollisionSensitivity(int robot_id, const int* joint_sensitivity)
{
    auto cmd = std::make_shared<SafetyCmdSetCollisionSensitivity>(robot_id, joint_sensitivity);
    return mot::realtime_cmd_call(cmd);
}

// cmd 设置 --- collision
int zucSetCollisionDetectionEnable(int robot_id, int collision_detection_enable)
{
    auto cmd = std::make_shared<CmdSetCollisionDetectionEnable>(robot_id, collision_detection_enable);
    return mot::realtime_cmd_call(cmd);
}

// 锁 设置 --- collision
int zucSetCollisionOption(int robot_id,
                          int reactionType,
                          double reboundAngle,
                          double reboundLength,
                          double rebound_j_acc,
                          double rebound_j_vel,
                          double rebound_l_acc,
                          double rebound_l_vel,
                          int collisionMethod)
{
    if (check_config_active(robot_id))
    {
        return -1;
    }
    if (reboundAngle < 0 || reboundAngle > 3 || reactionType < 0 || reactionType > 1 || collisionMethod < 0)
    {
        TASKCONFIG_ERR("Invalid collision option %d %f %d\n", reactionType, reboundAngle, collisionMethod);
        return -1;
    }

    TASKCONFIG_INFO("ZUCMOT_SET_COLLISION_OPTION: %d %f %f %f %f %f %f %d\n",
                    reactionType,
                    reboundAngle,
                    reboundLength,
                    rebound_j_acc,
                    rebound_j_vel,
                    rebound_l_acc,
                    rebound_l_vel,
                    collisionMethod);
    auto set_intf = axisgroup::AxisGroupMgr::instance().get_axisgroup(robot_id)->get_set_intf();
    auto config = set_intf->get_usr_config();

    {
        std::lock_guard<std::mutex> lock(set_intf->config_mutex_);
        config->robot_safety.collision.collision_method = collisionMethod;
        config->robot_safety.collision.rebound_angle = reboundAngle;
        config->robot_safety.collision.rebound_length = reboundLength;
        config->robot_safety.collision.rebound_j_acc = rebound_j_acc;
        config->robot_safety.collision.rebound_j_vel = rebound_j_vel;
        config->robot_safety.collision.rebound_l_acc = rebound_l_acc;
        config->robot_safety.collision.rebound_l_vel = rebound_l_vel;
        config->robot_safety.collision.reaction_type = reactionType;
        set_intf->config_change();
    }

    return 0;
}

int zucTaskSetAutoWork(int robot_id, int enable, const char* file)
{
    //    int ret;
    //    zucStatus->task.autoWork.enable = enable;
    //    strncpy(zucStatus->task.autoWork.filename, file, 256);
    //    rtapi_print("[task] auto work filename: %s\n", file);
    //
    //    auto cmd = std::make_shared<CmdSetAutoWork>(robot_id, file, enable);
    //    ret = mot::realtime_cmd_call(cmd);
    //
    //    if (task_init_finish_flag)
    //    {
    //        usrmotReadZucmotStatus(&zucmotStatus);
    //        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    //    }
    //    return ret;
    return 0;
}

int zucSetPayloadIdentifyTrajStartPos(int robot_id, int trajId, JointTraj jointTraj)
{
    //    if (trajId < 0 || trajId > 3)
    //        return -1;
    //    std::string msg = "IdentifyTraj0 " + std::to_string(trajId) + ":start ";
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { msg += std::to_string(jointTraj.startJntPos[i]) + ", "; }
    //    msg + "\n";
    //    msg = "IdentifyTraj0" + std::to_string(trajId) + ":end ";
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { msg += std::to_string(jointTraj.startJntPos[i]) + ", "; }
    //    msg + "\n";
    //    zlog()->info(msg);
    //
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucStatus->task.payloadIdentifyTraj[trajId].startJntPos[i] = jointTraj.startJntPos[i]; }
    //
    //    return 0;
    return 0;
}

int zucStopIdentifyFriction(int robot_id, int friction_identify_joint_index)
{
    //    auto cmd = std::make_shared<CmdStopIdentifyFriction>(robot_id);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetIdentifyFrictionEndPos(int robot_id, int friction_identify_joint_index, const double* friction_identify_end_pos)
{
    //    auto cmd = std::make_shared<CmdSetIdentifyFrictionEndPos>(robot_id, friction_identify_joint_index, friction_identify_end_pos);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetTioRs485SigProgStat(int robot_id, const char* sig_name, int prog_stat)
{
    //    if (sig_name == NULL || strlen(sig_name) > 128 || prog_stat > 1 || prog_stat < 0)
    //        return -1;
    //    auto cmd = std::make_shared<CmdSetTioRs485SigProgStat>(robot_id, sig_name, prog_stat);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetGravityDirection(int robot_id, const double* rpy)
{
    //    auto cmd = std::make_shared<CmdSetGravityDirection>(robot_id, rpy);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetDragControlFcCompensate(int robot_id, const int* drag_control_fc_compensate)
{
    //    auto cmd = std::make_shared<CmdSetDragControlFcCompensate>(robot_id, drag_control_fc_compensate);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetDragControlFilterFactor(int robot_id, const int* drag_control_filter_factor)
{
    //    auto cmd = std::make_shared<CmdSetDragControlFilterFactor>(robot_id, drag_control_filter_factor);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetIdentifyCycleTime(int robot_id, int identify_cycle_time)
{
    //    if (identify_cycle_time < 20)
    //    {
    //        identify_cycle_time = 20;
    //    }
    //    else if (identify_cycle_time > 500)
    //    {
    //        identify_cycle_time = 500;
    //    }
    //    auto cmd = std::make_shared<CmdSetIdentifyCycleTime>(robot_id, identify_cycle_time);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajSetRobotToolOffset(ZucPose tool_offset)
{
    //    rcs_print("zucTrajSetRobotToolOffset tool offset: %f, %f, %f, %f, %f, %f\n",
    //              tool_offset.tran.x,
    //              tool_offset.tran.y,
    //              tool_offset.tran.z,
    //              tool_offset.a,
    //              tool_offset.b,
    //              tool_offset.c);
    //    zucStatus->task.robotToolOffset = tool_offset;
    //    zucStatus->motion.currentToolId = -1;
    //    auto cmd = std::make_shared<CmdSetRobotToolId>(robot_id, tool_offset);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajSetRobotUserOffset(int robot_id, ZucPose user_offset)
{
    //    zucStatus->task.robotUserOffset = user_offset;
    //    zucStatus->motion.currentUserId = -1;
    //    auto cmd = std::make_shared<CmdSetRobotUserId>(robot_id, user_offset);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajSetPlannerJerk(int robot_id, double joint_jerk, double carte_jerk)
{
    //    double jerk1, jerk2;
    //    jerk1 = joint_jerk;
    //    jerk2 = carte_jerk;
    //
    //    zucSetPlannerJerk(jerk1, jerk2);
    //    return 0;
    return 0;
}

int zucSetupScriptBackup(int robot_id, int enable, double interval)
{
    //    if (interval >= 0)
    //    {
    //        zucStatus->task.backupSetting.enable = enable;
    //        zucStatus->task.backupSetting.interval = interval;
    //        return 0;
    //    }
    //    else
    //    {
    //        rcs_print("invalid backup interval\n");
    //        return -1;
    //    }
    return 0;
}

int zucTorqSensorSetComm(int robot_id, int commType, CommInfo commCfg)
{
    //    auto cmd = std::make_shared<CmdSetcommTorqsensor>(robot_id, commCfg, commType);
    //    return mot::realtime_cmd_call(cmd);
    //    return 0;
    return 0;
}

int zucToolSetPayload(int robot_id, Payload payload)
{
    //    zucStatus->payload[zucStatus->motion.cur_payload_id] = payload;
    //
    //    auto cmd = std::make_shared<CmdSetpayloadTool>(robot_id, payload);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTorqSensorSetMode(int robot_id, int mode)
{
    //    zucStatus->motion.sensor.sensorConfig.mode = mode;
    //
    //    auto cmd = std::make_shared<CmdSetmodeTorqsensor>(robot_id, mode);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTioMBAddSignal(int robot_id, const char* name, int chnId, unsigned int type, unsigned int addr)
{
    //    if (name == NULL || strlen(name) > 128 || type > 4)
    //        return -1;
    //
    //    zucmotCommand.command = ZUCMOT_MBTIO_ADD_SIGNAL;
    //    strcpy(zucmotCommand.tioplsAddSig.sigName, name);
    //    zucmotCommand.tioplsAddSig.chnId = chnId;
    //    zucmotCommand.tioplsAddSig.type = type;
    //    zucmotCommand.tioplsAddSig.addr = addr;
    //    rcs_print("signal: %s, %d, %d, %d\n", name, chnId, type, addr);
    //    int res = usrmotWriteZucmotCommand(&zucmotCommand);
    //    // if (res == 0 && saveOpt)
    //    // {
    //    //     usrmotReadZucmotStatus(&zucmotStatus);
    //    //     for (int i = 0; i < MAX_TIO_SIG_NUM; ++i) { zucmotStatus.gripperState[i].freq = gripperFreq[i]; }
    //    //     xmlSaveTIOSignals(MAX_TIO_SIG_NUM, zucmotStatus.gripperState);
    //    // }
    //    return res;
    return 0;
}

int zucTioSetupRS485ChnMode(int robot_id, int chnId, int mode)
{
    //    if (chnId > 1 || chnId < 0)
    //        return -1;
    //
    //    auto cmd = std::make_shared<CmdTioSetRs485ChnMode>(robot_id, mode, chnId);
    //    int res = mot::realtime_cmd_call(cmd);
    //
    //    // if (res == 0 && saveOpt)
    //    // {
    //    //     usrmotReadZucmotStatus(&zucmotStatus);
    //    //     xmlSaveTIOSignals(MAX_TIO_SIG_NUM, zucmotStatus.gripperState);
    //    //     iniSaveTioRS485ChnMode(chnId, mode, zuc_inifile);
    //    // }
    //    return res;
    return 0;
}

int zucTioSetPinMode(int robot_id, int type, int mode)
{
    //    if (type > 2 || type < 0)
    //        return -1;
    //
    //    // rtuComm.slaveId = 0;
    //    // rtuComm.baudrate = 115200;
    //    // rtuComm.databits = 8;
    //    // rtuComm.stopbits = 1;
    //    // rtuComm.parity = 78;
    //    // zucmotConfig.tioConfig.rsChnCfg[type - 1].rtuComm = rtuComm;
    //    // iniSaveTioRS485ChnComm(type - 1, rtuComm, zuc_inifile);
    //
    //    auto cmd = std::make_shared<CmdTioSetPinMode>(robot_id, mode, type);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTioSetVoutVoltage(int robot_id, int voltage)
{
    //    auto cmd = std::make_shared<CmdTioSetVoutVol>(robot_id, voltage);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucMotionFreeButtonOpt(int robot_id, bool freeButtonOpt)
{
    //    auto cmd = std::make_shared<CmdSetFreeButtonOpt>(robot_id, freeButtonOpt);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucMotionFreeProgButtonOpt(int robot_id, bool freeButtonOpt, bool progButtonOpt)
{
    //    auto cmd = std::make_shared<CmdSetFreeProgButtonOpt>(robot_id, freeButtonOpt, progButtonOpt);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucOneKeyOp(int robot_id, int autoenable, int autoprogram)
{
    //    int ret;
    //    if (autoenable == 0 || autoenable == 1)
    //    {
    //        zucStatus->autoopsetting.onekeyenable = autoenable;
    //    }
    //    else
    //    {
    //        rcs_print("the autoenable value can only set to be 0 or 1\n");
    //    }
    //    if (autoprogram == 0 || autoprogram == 1)
    //    {
    //        zucStatus->autoopsetting.autoprogram = autoprogram;
    //    }
    //    else
    //    {
    //        rcs_print("the autoprogram value can only set to be 0 or 1\n");
    //        return -1;
    //    }
    //    if (task_init_finish_flag && !zucStatus->motion.maintenance_status)
    //    {
    //        usrmotReadZucmotStatus(&zucmotStatus);
    //        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    //    }
    //    auto cmd = std::make_shared<CmdSetOneKeyOp>(robot_id, autoprogram, autoenable);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetupIOChannel(int robot_id, int type, int index, int setting)
{
    //    if ((type == 0 && zuc_cab_type != 2) || (type == 1 && zuc_cab_type != 3))
    //    {
    //        zucOperatorError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function");
    //        return -1;  // cab v2.1 support analog setup, mini cab support digital setup
    //    }
    //
    //    auto cmd = std::make_shared<CmdSetupIoChannel>(robot_id, index, type, setting);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetRobotName(int robot_id, const char* name)
{
    //    if (name == nullptr)
    //    {
    //        return -1;
    //    }
    //    if (has_special_chars(std::string(name)))  // 不可以有特殊字符)
    //    {
    //        int robot_id = -1;
    //        rcs_print("zucSetRobotName(robot_id, %s): paramater check [ FAILED ] \n", name);
    //        return -1;
    //    }
    //    strcpy(zucStatus->robotName, name);
    //    return 0;
    return 0;
}

int zucSetPayloadIdentifyTrajEndPos(int robot_id, int trajId, JointTraj jointTraj)
{
    //    if (trajId < 0 || trajId > 3)
    //        return -1;
    //
    //    std::string msg = "IdentifyTraj1 " + std::to_string(trajId) + ":start ";
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { msg += std::to_string(jointTraj.endJntPos[i]) + ", "; }
    //    msg + "\n";
    //    msg = "IdentifyTraj1" + std::to_string(trajId) + ":end ";
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { msg += std::to_string(jointTraj.endJntPos[i]) + ", "; }
    //    msg + "\n";
    //    zlog()->info(msg);
    //
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucStatus->task.payloadIdentifyTraj[trajId].endJntPos[i] = jointTraj.endJntPos[i]; }
    //
    //    return 0;
    return 0;
}

int zucSetIdentifyFrictionStartPos(int robot_id, int friction_identify_joint_index, const double* friction_identify_start_pos)
{
    //    zucStatus->motion.friction_identify_joint_index = friction_identify_joint_index;
    //    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    //    {
    //        zucStatus->motion.friction_identify_start_pos[i + friction_identify_joint_index * zucStatus->motion.robot_joint_num] = friction_identify_start_pos[i];
    //    }
    //    auto cmd = std::make_shared<CmdSetIdentifyFrictionStartPos>(robot_id, friction_identify_joint_index, friction_identify_start_pos);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetForceLimitValue(int robot_id, int force_limit_value)
{
    //    if (!collision_detect_flag)
    //    {
    //        return 0;
    //    }
    //    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_ADMITTANCE || zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_DRAG ||
    //        zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_SERVOJOP)
    //    {
    //        zucOperatorError(CANNOT_SET_COLLISION_SENSITIVITY, "cannot set collision sensitivity in drag mode or force control mode");
    //        return -1;
    //    }
    //
    //    auto cmd = std::make_shared<CmdSetForceLimitValue>(robot_id, force_limit_value);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetDragControlEnable(int robot_id, int drag_control_enable)
{
    //    auto cmd = std::make_shared<CmdSetDragControlEnable>(robot_id, drag_control_enable);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetDragControlDitherFactor(int robot_id, const double* drag_control_dither_factor)
{
    //    auto cmd = std::make_shared<CmdSetDragControlDitherFactor>(robot_id, drag_control_dither_factor);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajSetFtCondition(int robot_id, double* compliantCondition)
{
    //    for (int i = 0; i < CART_DIM; i++)
    //    {
    //        zucStatus->motion.compliantCondition[i] = compliantCondition[i];
    //        // zucmotCommand.compliantCondition[i] = compliantCondition[i];
    //        //rcs_print("compliantCondition[%d] set: %f\n", i, compliantCondition[i]);
    //    }
    //
    //    auto cmd = std::make_shared<CmdSetFtCompliantCondition>(robot_id, compliantCondition);
    //    return mot::realtime_cmd_call(cmd);
    //}
    return 0;
}

int zucTorqSensorSetPayload(int robot_id, Payload payload)
{
    //    // 无意义
    //    // zucStatus->motion.sensor.sensorConfig.payload = payload;
    //
    //    auto cmd = std::make_shared<CmdSetPayloadTorqsensor>(robot_id, payload);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTioSetupRS485ChnComm(int robot_id, int chnId, ModRtuComm& rtuComm)
{
    //    if (chnId > 1 || chnId < 0)
    //        return -1;
    //
    //    auto cmd = std::make_shared<CmdTioSetRs485ChnComm>(robot_id, rtuComm, chnId);
    //    return mot::realtime_cmd_call(cmd);
    //    return 0;
    return 0;
}

int zucMotionPointButtonOpt(int robot_id, bool pointButtonOpt)
{
    //    auto cmd = std::make_shared<CmdSetPointButtonOpt>(robot_id, pointButtonOpt);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetBRStartVoltage(int robot_id, int start_voltage)
{
    //    if (zuc_cab_type != 3)
    //    {
    //        zucOperatorError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function");
    //        return -1;  // only mini cab support digital setup
    //    }
    //    int scb_br_start_voltage = 0;
    //    if (start_voltage < 20)
    //        scb_br_start_voltage = 20;
    //    else if (start_voltage > 70)
    //        scb_br_start_voltage = 70;
    //    else
    //        scb_br_start_voltage = start_voltage;
    //
    //    auto cmd = std::make_shared<CmdSetBrStartVoltage>(robot_id, scb_br_start_voltage);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetDragControlFvCompensate(int robot_id, const int* drag_control_fv_compensate)
{
    //    auto cmd = std::make_shared<CmdSetDragControlFvCompensate>(robot_id, drag_control_fv_compensate);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTioSetVoutEnable(int robot_id, int enable)
{
    //    auto cmd = std::make_shared<CmdTioSetVoutEna>(robot_id, enable);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetServoMoveLAVSParamC(int robot_id, int maxSize, double kp)
{
    //    TrajConfig.mmf_buf_size = maxSize;
    //    TrajConfig.mmf_kp = kp;
    //    return 0;
    return 0;
}

int zucTorqSensorSetBrand(int robot_id, int torqsensorBrand)
{
    //    zucStatus->motion.sensor.sensorConfig.torqsensorBrand = torqsensorBrand;
    //
    //    auto cmd = std::make_shared<CmdSetbrandTorqsensor>(robot_id, torqsensorBrand);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}
